#include "myrealize.hpp"

void toHsvBin(cv::InputArray RGB_raw,cv::OutputArray RGB_bin,bool flag)
{
    Mat temp;
    cv::cvtColor(RGB_raw, temp, COLOR_BGR2HSV);
    if(flag == red)
    {
        cv::inRange(temp, Scalar(0,43,150), Scalar(10,255,255), temp);
    }
    else
    {
        //78~99 is cyan and 100~124 is blue.
        //reference to ../build/hsv_define.png.
        cv::inRange(temp, Scalar(70,43,150), Scalar(140,255,255), temp);
    }
    RGB_bin.create(temp.size(),temp.type());
    temp.copyTo(RGB_bin.getMat());
    return ;
}

void contoursSelect(cv::OutputArray Picture_raw,cv::InputArray Picture_binary,cv::OutputArray contours,cv::OutputArrayOfArrays contours_)
{
    vector<Vec4i> hierarchy;
    vector<Mat> temp;
    vector<vector<Point>> con;

    // You can get the contours which is selected by CHAIN_APPROX_NONE algorithm and The contours is organized by the RETR_CCOMP way.
    // when you are contouring,The Input of the findcontours function must be an binary Mat.
    cv::findContours(Picture_binary,contours,hierarchy,RETR_CCOMP,CHAIN_APPROX_NONE);
    contours.getMatVector(temp);
    // get the selective contour's index
    vector<int> indexs;

    for(int i = 0; i < temp.size(); i++)
    {
        RotatedRect rotate_Rect;
        // Rect rect;
        float contour_area = contourArea(temp[i]);
        /**************** condition contour area *****************
         *********************************************************/
        if(contour_area < 0 || contour_area > 300) continue;
        // rect = boundingRect(temp[i]);
        rotate_Rect = minAreaRect(temp[i]);
        float width = rotate_Rect.size.width;                 //旋转矩阵的宽度
        float height = rotate_Rect.size.height;               //旋转矩阵的高度
        float rate_w_h;
        if(width > height)
        {
            rate_w_h = width/height;
        }
        else
        {
            rate_w_h = height/width;
        }
        /**************** condition the rate of height and width *******
         ***************************************************************/
        if(rate_w_h < 0.5 || rate_w_h > 5) continue;
        float angle = abs(rotate_Rect.angle);
        /**************** condition the angle of the rotate rectangle***
         ***************************************************************/
        if(angle > 30 && angle < 70) continue;
        indexs.push_back(i);
        con.push_back(temp[i]);
        drawRotateRect(Picture_raw.getMat(), rotate_Rect);
        /******************************************code block for debug
         * "(" + convFloatToSring(getRotateRectCenter(rotate_Rect).x) + "," + 
         * Point center = getRotateRectCenter(rotate_Rect);
         * center.y = center.y + 10;
         * putText(Picture_raw.getMat(),convFloatToSring(getRotateRectCenter(rotate_Rect).x),getRotateRectCenter(rotate_Rect),FONT_HERSHEY_SIMPLEX,0.4,Scalar(255,255,255));
         * putText(Picture_raw.getMat(),convFloatToSring(angleNormalization(rotate_Rect.angle)),center,FONT_HERSHEY_SIMPLEX,0.4,Scalar(255,255,255));
        *****************************************************************/
    }

    // When I construct a vector<vector<Point>>,I have to clear it out because of the segement fault.
    contours_.clear();
    contours_.create(indexs.size(), 1, 0, -1, true);
    for(int i = 0; i < indexs.size(); i++)
    {
        contours_.create(temp[indexs[i]].size(), CV_32SC2, i, true);
        temp[indexs[i]].copyTo(contours_.getMat(i));
    }
}

float angleNormalization(float angle)
{
    angle = abs(angle);
    if(angle > 45)
    {
        angle = 90 - angle;
    }
    return angle;
}

void matchLightBar(cv::OutputArray Picture_raw,cv::OutputArray contours,vector<armorPiece> &armors)
{
    Point center1,center2;
    vector<Mat> contours_;
    contours.getMatVector(contours_);

    // when the flag is set to true,it represents it has been selected.
    bool flags[contours_.size()];
    memset(flags,false,contours_.size()*sizeof(bool));
    if(contours_.size() < 2) return ;

    //筛选出来的灯条进行左右配对
    for(int i = 0; i < contours_.size(); i++)
    {
        if(flags[i] == true) continue;
        for(int j = i+1; j < contours_.size(); j++)
        {
            if(flags[j] == true) continue;
            // debug用 cout<<abs(rotate_Rect_Group[i].angle - rotate_Rect_Group[j].angle)<<endl;
            // 得到兩個旋轉矩形的中心點
            RotatedRect rotate_Rect1,rotate_Rect2;
            Rect rect1,rect2;
            float area1,area2;
            rect1 = boundingRect(contours_[i]);
            rect2 = boundingRect(contours_[j]);
            rotate_Rect1 = minAreaRect(contours_[i]);
            rotate_Rect2 = minAreaRect(contours_[j]);
            area1   = rotate_Rect1.size.area();
            area2   = rotate_Rect2.size.area();

            if(abs(area1-area2) > 30) continue;

            center1 = getRotateRectCenter(rotate_Rect1);
            center2 = getRotateRectCenter(rotate_Rect2);
            if(abs(center1.y - center2.y) > 10) continue;
            if(abs(center1.x - center2.x) < 10 || abs(center1.x - center2.x) > 300) continue;

            float angle_i = angleNormalization(rotate_Rect1.angle);
            float angle_j = angleNormalization(rotate_Rect2.angle);
            if(abs(angle_i - angle_j) > 15.) continue;

            armorPiece temp;
            temp.initial(rotate_Rect1,rotate_Rect2);
            temp.initial(rect1,rect2);
            
            // 根据装甲片性质二次筛选
            if(temp.width/temp.height > 9) continue;
            flags[i] = true,flags[j] = true;
            temp.drawArmor(Picture_raw);
            armors.push_back(temp);


            /******************************************code block for debug
             * temp.drawArmor(Picture_raw.getMat());
             * drawRotateRect(src,rotate_Rect_Group[i]);
             * drawRotateRect(src,rotate_Rect_Group[j]);
             * circle(Picture_raw.getMat(), center1, 5, Scalar(255,255,255), -1, -1);
             * circle(Picture_raw.getMat(), center2, 5, Scalar(255,255,255), -1, -1);
             * circle(Picture_raw.getMat(), Point((center1.x+center2.x)/2,(center1.y+center2.y)/2),10,Scalar(255,255,255), -1, -1);
            ****************************************************************/
        }
    }
}

void sortArmors(vector<armorPiece> &armors)
{
    for(int i = armors.size() - 1; i > 0; i--)
    {
        for(int j = 0; j < i; j++)
        {
            if(armors[j].score < armors[j+1].score)
            {
                armorPiece temp = armors[j];
                armors[j]       = armors[j+1];
                armors[j+1]     = temp;
            }
        }
    }
}